/*
 * navigation.cpp
 *
 *  Created on: Nov 19, 2012
 *      Author: Matthijs
 */
#include <behavior/navigation.h>
#include <math.h>


Navigation::Navigation (ros::NodeHandle n) :
	nodeHandle(n),
	tags(),
	targetInitialized(false),
	goingToStart(false),
	takeSnapshot(false)
{
	priority 		= 0.3;
	active 			= true;
	location_client = n.serviceClient<SLAM::Location>("/slam/location");
	tag_client		= n.serviceClient<SLAM::RecordedTags>("/SLAM/tags");
	waypoint_client = n.serviceClient<SLAM::Waypoints>("/SLAM/waypoints");
	snapshot_client = n.serviceClient<sensor::Snapshot>("/sensor/camera/snapshot");
	targetInitialized = false;
	ROS_INFO("Navigation behavior ready.");
}


void Navigation::setActive (bool state)
{
	active = state;
	if (active == false)
		return;

	// We just got activated. First, erase any old tags.
	tags.clear();

	// Now, load a NEW list of tags, then go down the list.
	SLAM::RecordedTags tag_info;
	tag_client.call(tag_info);
	for (uint i = 0; i < tag_info.response.tags.size(); i++) {
		Tag t;
		t.x = tag_info.response.tags[i].x;
		t.y = tag_info.response.tags[i].y;
		t.index = tag_info.response.tags[i].index;
		tags.push_back(t);
	}

	loadWaypointsForTag(tags[0].index);
}


/**
 * 	Request a list of waypoints for the first tag in our tags list.
 *
 * 	We do this by calling the waypoint service in the SLAM package.
 */
void Navigation::loadWaypointsForTag (int tagIndex)
{
	SLAM::Waypoints waypoint_info;
	waypoint_info.request.index = tagIndex;
	if (!waypoint_client.call(waypoint_info)) {
		ROS_ERROR("Navigation failed waypoints client call");
	}
	waypoints.clear();
	for (uint i = 0; i < waypoint_info.response.waypoints.size(); i++) {
		Waypoint wp;
		wp.x = waypoint_info.response.waypoints[i].x;
		wp.y = waypoint_info.response.waypoints[i].y;
		waypoints.push_back(wp);
	}

	ROS_INFO("Loaded %d waypoints for tag %d.", waypoints.size(), tags[0].index);
}


/**
 * 	Call the SLAM location service to get an up-to-date fix on our position
 */
void Navigation::updateLocation ()
{
	SLAM::Location location;
	location_client.call(location);
	x = location.response.x;
	y = location.response.y;
	angle = location.response.angle;
	ROS_INFO("Updated location: %f, %f, heading %f", x, y, angle);
}


/**
 * 	Select the current waypoint to manouvre to. This includes
 * 	clearing waypoints if we are close enough to them and
 * 	moving on to the next waypoint.
 *
 * 	For this, we need an up-to-date location before calling this
 * 	method.
 */
bool Navigation::selectWaypoint ()
{
	bool result = true;
	if (!targetInitialized)
		result = selectNextWaypoint();

	targetInitialized = true;
	float dx = target.x - x;
	float dy = target.y - y;

	if (sqrtf(dx * dx + dy * dy) < 0.10f) {
		ROS_ERROR("Selecting next waypoint");
		result = selectNextWaypoint();
	}
	return result;
}


/**
 * 	Select the first waypoint, set it as our target, and remove it
 * 	from the list of waypoints.
 */
bool Navigation::selectNextWaypoint ()
{
	bool result = true;
	if (waypoints.size() == 0)
	{
		if (tags.size() > 0) {
			tags.erase(tags.begin());
			loadWaypointsForTag(tags[0].index);
		} else {
			if (goingToStart) {
				return false;
			} else {
				// Load waypoints for start position
				loadWaypointsForTag(-1);
			}
			goingToStart = true;
		}
		result = false;
	}

	// No more new waypoints. Return.
	if (waypoints.size() == 0)
		return false;

	target = waypoints[0];
	waypoints.erase(waypoints.begin());
	ROS_INFO("Next waypoint selected. %d waypoints left.", waypoints.size());
	return result;
}


/**
 *  Move in the direction of our current target.
 */
void Navigation::moveToTarget (driver::Move * move)
{
	float vx = target.x - x;
	float vy = target.y - y;
	float target_dir = atan2f(vy, vx);
	float dist = sqrtf(vx * vx + vy * vy);

	move -> wait_until_completion 	= false;
	move -> distance 				= dist;
	move -> angle 					= target_dir - angle;

	if (fabsf(target_dir - angle) > 0.1)
	{
		move -> wait_until_completion = true;
		move -> distance = 0.0;
	}
}


/**
 * 	Stand still. This allows us to take a good quality snapshot.
 */
void Navigation::standStill (driver::Move * move)
{
	move -> distance 				= 0.0f;
	move -> angle 					= 0.0f;
}


/**
 * 	Take a camera snapshot. Call the camera snapshot service, and stop
 * 	moving for the next iteration.
 */
void Navigation::takeCamshot ()
{
	sensor::Snapshot snapshot;
	snapshot.request.left 	= true;
	snapshot.request.right 	= true;
	snapshot_client.call(snapshot);
}


/**
 *  Navigation Act:
 *
 *  Move in the direction suggested by our path planner.
 */
bool Navigation::act (ros::Publisher publisher)
{
	driver::Move move;
	updateLocation();

	if (takeSnapshot)
	{
		ROS_INFO("Standing still (%f, %f) and taking a snapshot.", move.distance, move.angle);
		publisher.publish(move);
		takeCamshot();
		takeSnapshot = false;
		return true;
	}

	if (selectWaypoint())			// There was a next waypoint on the list. Move to it.
	{
		moveToTarget(&move);
		ROS_INFO("Moving to target (%f, %f): %f, %f", target.x, target.y, move.distance, move.angle);
		publisher.publish(move);
	} else {						// We reached the last waypoint. Stand still, take a picture.
		standStill(&move);
		takeSnapshot = true;
	}
	return true;
}
